#include <arm_math.h>
#include <math.h>
#include "./Bezier.hpp"
#include "./My_Math.hpp"
#include "../parameter.h"

float Bezier_Curve_Class::curve_u;      /* 小数部分为曲线参数u */
float Bezier_Curve_Class::curve_length; /* 参数u对应的曲线长度 */

union Bezier_Curve_Class::Bezier_Control_Coor Bezier_Curve_Class::target_coor; /* 目标坐标 */
union Bezier_Curve_Class::Bezier_Control_Coor Bezier_Curve_Class::target_v;    /* 目标速度方向 */

union Bezier_Curve_Class::Bezier_Control_Coor operator+(const union Bezier_Curve_Class::Bezier_Control_Coor &A,
                                                        const union Bezier_Curve_Class::Bezier_Control_Coor &B)
{
    union Bezier_Curve_Class::Bezier_Control_Coor temp;
    temp.x = A.x + B.x;
    temp.y = A.y + B.y;
    temp.z = A.z + B.z;
    return temp;
}

union Bezier_Curve_Class::Bezier_Control_Coor operator-(const union Bezier_Curve_Class::Bezier_Control_Coor &A,
                                                        const union Bezier_Curve_Class::Bezier_Control_Coor &B)
{
    union Bezier_Curve_Class::Bezier_Control_Coor temp;
    temp.x = A.x - B.x;
    temp.y = A.y - B.y;
    temp.z = A.z - B.z;
    return temp;
}

union Bezier_Curve_Class::Bezier_Control_Coor operator*(const union Bezier_Curve_Class::Bezier_Control_Coor &A, const float factor)
{
    union Bezier_Curve_Class::Bezier_Control_Coor temp;
    temp.x = A.x * factor;
    temp.y = A.y * factor;
    temp.z = A.z * factor;
    return temp;
}

inline float Bezier_Curve_Class::Cal_1Norm(const union Bezier_Curve_Class::Bezier_Control_Coor &A)
{
    return My_Math_Class::ABS(A.x) + My_Math_Class::ABS(A.y) + My_Math_Class::ABS(A.z);
}

float Bezier_Curve_Class::Cal_2Norm(const union Bezier_Curve_Class::Bezier_Control_Coor &A)
{
    float norm;
    norm = A.x * A.x + A.y * A.y + A.z * A.z;
    norm = sqrtf(norm);
    return norm;
}

float Bezier_Curve_Class::Generate_Curve(const union Bezier_Control_Coor *const _P1)
{
    P1 = _P1; /* 保存第一个控制点 */

    union Bezier_Control_Coor v[2]; /* 起点终点速度 */
    if (nullptr == pre_curve)
    {
        v[0] = P4 - *P1;   /* 第一段曲线，速度应从当前起点指向当前终点 */
        target_v = v[0];   /* 保存初始速度方向 */
        target_coor = *P1; /* 保存初始目标坐标 */
    }
    else
    {
        v[0] = P4 - *(pre_curve->P1); /* 不是第一段曲线，速度应从上一段曲线的起点指向当前终点 */
    }

    if (nullptr == next_curve)
    {
        v[1] = P4 - *P1; /* 最后一段曲线，速度应从当前起点指向当前终点 */
    }
    else
    {
        v[1] = next_curve->P4 - *P1; /* 不是最后一段曲线，速度应从当前起点指向下一段曲线终点 */
    }

    float k = 1.0f;

    /* 根据矩阵范数相容性，缩放速度 */
    for (int i = 0; i < 2; i++)
    {
        float norm = Cal_1Norm(v[i]);
        if (norm > agv_parameter->max_velocity_line)
        {
            k = agv_parameter->max_velocity_line / norm;
        }
        else
        {
            k = 1.0f;
        }
        v[i] = v[i] * k;
    }

    union Bezier_Control_Coor coor_temp = P4 - *P1; /* 计算始末距离 */
    float distance_temp = Cal_1Norm(coor_temp);     /* 计算距离 */

    if (FP_ZERO == fpclassify(distance_temp))
    {
        /* 始末点重合 */
        P2 = *P1;
        P3 = P4;
    }
    else
    {
        float v_norm;
        float gain;
        v_norm = Cal_1Norm(v[0]);
        gain = distance_temp / v_norm * (1 / 4.0f);
        P2 = *P1 + (v[0] * gain);

        v_norm = Cal_1Norm(v[1]);
        gain = distance_temp / v_norm * (1 / 4.0f);
        P3 = P4 - (v[1] * gain);
    }
    /* 计算各多项式系数 */
    ka = *P1 * (-3.0f) + P2 * (9.0f) + P3 * (-9.0f) + P4 * (3.0f);
    kb = *P1 * (6.0f) + P2 * (-12.0f) + P3 * (6.0f);
    kc = *P1 * (-3.0f) + P2 * (3.0f);

    /* 将P2，P3放大3倍，便于计算 */
    P2 = P2 * 3.0f;
    P3 = P3 * 3.0f;

    if (FP_ZERO == fpclassify(distance_temp))
    {
        for (int i = 0; i < 3; i++)
        {
            range[i].minor.interval = 0.0f;
            range[i].larger.interval = 1.0f;
        }
    }
    else
    {
        for (int i = 0; i < 3; i++)
        {
            Cal_Range(ka.coor[i], kb.coor[i], kc.coor[i], range[i]); /* 计算区间 */
        }
    }

    coor_temp = Func_Bezier(0.0f);
    for (int i = 0; i < 3; i++)
    {
        range[i].left_value = coor_temp.coor[i];
    }
    coor_temp = Func_Bezier(1.0f);
    for (int i = 0; i < 3; i++)
    {
        range[i].right_value = coor_temp.coor[i];
    }
    for (int i = 0; i < 3; i++)
    {
        coor_temp = Func_Bezier(range[i].minor.interval);
        range[i].minor.value = coor_temp.coor[i];
    }
    for (int i = 0; i < 3; i++)
    {
        coor_temp = Func_Bezier(range[i].larger.interval);
        range[i].larger.value = coor_temp.coor[i];
    }

    sum_length = 0.0f;

    for (int i = 0; i < 3; i++)
    {
        interval_length[i].left_length = My_Math_Class::ABS(range[i].left_value - range[i].minor.value);
        interval_length[i].middle_length = My_Math_Class::ABS(range[i].minor.value - range[i].larger.value);
        interval_length[i].right_length = My_Math_Class::ABS(range[i].larger.value - range[i].right_value);
        sum_length += interval_length[i].left_length + interval_length[i].middle_length + interval_length[i].right_length;
    }

    return sum_length;
}

bool Bezier_Curve_Class::Cal_Target(const union Bezier_Control_Coor &current_coor)
{
    union Bezier_Control_Coor projection_vector; /* 投影向量 */

    if (FP_ZERO == fpclassify(sum_length))
    {
        /* 当前曲线长度为0 */
        return false;
    }

    target_coor = Func_Bezier(curve_u);   /* 计算目标坐标 */
    target_v = Func_Bezier_Diff(curve_u); /* 计算目标速度矢量方向 */

    /* 迭代5次 */
    for (int i = 0; i < 5; i++)
    {
        projection_vector = current_coor - target_coor; /* 计算待投影向量 */
        float X_H_mul_X = target_v.x * target_v.x +
                          target_v.y * target_v.y +
                          target_v.z * target_v.z; /* 计算目标投影向量 */

        float X_H_mul_y = projection_vector.x * target_v.x +
                          projection_vector.y * target_v.y +
                          projection_vector.z * target_v.z; /* 切向量为速度矢量 */

        float u_delta = 0.0f;
        if (FP_ZERO == fpclassify(X_H_mul_X))
        {
            break; /* 认为当前投影向量即为当前向量 */
        }
        u_delta = X_H_mul_y / X_H_mul_X;
        if (FP_ZERO == fpclassify(u_delta))
        {
            break; /* 增量很小，满足条件 */
        }
        curve_u += (u_delta);

        target_coor = Func_Bezier(curve_u);   /* 计算目标坐标 */
        target_v = Func_Bezier_Diff(curve_u); /* 计算目标速度矢量方向 */
    }

    curve_length = Cal_Length(curve_u); /* 计算长度 */
    return curve_u <= 1.0f;
}

float Bezier_Curve_Class::Cal_Length(const float curve_u)
{
    float current_length = 0.0f;
    Bezier_Control_Coor temp = Func_Bezier(curve_u); /* 计算曲线参数对应的坐标 */
    for (int i = 0; i < 3; i++)
    {
        float current_value = temp.coor[i];
        /*在左区间*/
        if (curve_u < range[i].minor.interval)
        {
            current_length += My_Math_Class::ABS(current_value - range[i].left_value);
        }
        /*在中间区间*/
        else if (curve_u < range[i].larger.interval)
        {
            current_length += interval_length[i].left_length + My_Math_Class::ABS(current_value - range[i].minor.value);
        }
        /*在右区间*/
        else if (curve_u < 1.0f)
        {
            current_length += interval_length[i].left_length + interval_length[i].middle_length +
                              My_Math_Class::ABS(current_value - range[i].larger.value);
        }
        /* 曲线参数超出范围,继续计算*/
        else
        {
            current_length += interval_length[i].left_length + interval_length[i].middle_length +
                              interval_length[i].right_length + My_Math_Class::ABS(current_value - range[i].right_value);
        }
    }
    return current_length;
}

void Bezier_Curve_Class::Cal_Range(float a, float b, float c, integration_range_struct &range)
{
    /* 范围初值 */
    range.minor.interval = 0.0f;
    range.larger.interval = 1.0f;

    float delta = b * b - 4 * a * c;
    /*无实数解*/
    if (delta <= 0.0f)
    {
        return;
    }

    /*求两个实数解*/
    delta = sqrtf(delta);
    float x_larger = (-b + delta) / (2 * a);
    float x_minor = (-b - delta) / (2 * a);

    float x_temp;

    /*比较较大解和较小解大小*/
    if (x_larger < x_minor)
    {
        x_temp = x_minor;
        x_minor = x_larger;
        x_larger = x_temp;
    }

    /*如果较小解大于1*/
    if (x_minor >= 1.0f)
    {
        return;
    }
    else if (x_minor >= 0.0f)
    {
        range.minor.interval = x_minor;
    }

    if (x_larger <= 0.0f)
    {
        return;
    }
    else if (x_larger <= 1.0f)
    {
        range.larger.interval = x_larger;
    }
}

union Bezier_Curve_Class::Bezier_Control_Coor Bezier_Curve_Class::Func_Bezier(const float u)
{
    union Bezier_Control_Coor y;
    float b = 1.0f - u;
    y = *P1 * (b * b * b) + P2 * (b * b * u) + P3 * (b * u * u) + P4 * (u * u * u); /* 此处P2,P3已经放大三倍 */
    return y;
}

union Bezier_Curve_Class::Bezier_Control_Coor Bezier_Curve_Class::Func_Bezier_Diff(const float u)
{
    union Bezier_Control_Coor y;
    y = ka * (u * u) + kb * u + kc;
    return y;
}
